#include "lasercameratrajserver.h"
#include <qdebug.h>
#include <qthread.h>
#include "trajorientationcalculate.h"
#include "messagelog.h"
#define D_MAX_QUEUE_SIZE 1000000
#define D_DELTA_MOVE 0.1
#define D_MAX_GAP 10

LaserCameraTrajServer::LaserCameraTrajServer(QString configFileIn, int robotIdIn, McControllerThread *programMcControllerIn)
{
    configFile=configFileIn;
    robotId=robotIdIn;
    programMcController=programMcControllerIn;
    isInitialed=false;
    isStart=false;

    trajOrientationCal=new TrajOrientationCalculate;

    if(1!=initialConfigFile(configFileIn))
    {

        addMsg( ENUM_MSG_ERROR,  "LaserCameraTrajServer", "LaserCameraAdapter",  12952,  robotId);

    }

}

int LaserCameraTrajServer::startBuffer()
{
    if(false==isInitialed)
    {
        createThread();
        isInitialed=true;
    }
    usingMutex.lock();
    bufferPointQueue.clear();
    usingMutex.unlock();
    isStart=true;
    return 1;
}

int LaserCameraTrajServer::stopBuffer()
{
//    qDebug()<<"--------stopBuffer in";
    isStart=false;
//    usingMutex.lock();
//    writeDataQueueToFile(bufferPointQueue,"/agvwork/buffer.csv");
//    usingMutex.unlock();
//    qDebug()<<"--------stopBuffer out";
    return 1;
}

int LaserCameraTrajServer::getTraj(QVector<PointPro > refOri,QVector<PointPro> &keyPointsOut)
{
    PointPro coordinatePoint;
    keyPointsOut.clear();
    programMcController->mcControllerInterpreter->getCurrentCoordinatePoint_Feedback(coordinatePoint);
    usingMutex.lock();
    for(int i=0;i<bufferPointQueue.size();i++)
    {
        coordinatePoint.positionValue[0]=bufferPointQueue[i].x;
        coordinatePoint.positionValue[1]=bufferPointQueue[i].y;
        coordinatePoint.positionValue[2]=bufferPointQueue[i].z;
        keyPointsOut.append(coordinatePoint);
    }
    usingMutex.unlock();
    if(refOri.size()>=3)
    {
        for(int i=0;i<refOri.size();i++)
        {
            if(JOINT_FLAG_JOINT==refOri[i].jointFlag)
            {
                addMsg( ENUM_MSG_ERROR,  "LaserCameraTrajServer", "LaserCameraAdapter",  12954,  robotId,
                        refOri[i].pointNum);
                return -1;
            }

            if(6>refOri[i].positionValue.size())
            {
                addMsg( ENUM_MSG_ERROR,  "LaserCameraTrajServer", "LaserCameraAdapter",  12955,  robotId,
                        refOri[i].pointNum);
                return -1;
            }

        }
        if(1!=trajOrientationCal->combineOrientation(refOri,keyPointsOut))
        {
            addMsg( ENUM_MSG_ERROR,  "LaserCameraTrajServer", "LaserCameraAdapter",  12953,  robotId);
            return -1;
        }
    }


    return 1;

}

int LaserCameraTrajServer::getLaserCameraBufferMoveConfigParameter(BufferMoveConfig &paraOut)
{
    paraOut=bufferMoveConfig;
    return 1;
}

int LaserCameraTrajServer::writeConfigFile(BufferMoveConfig bufferMoveConfigIn)
{
    return writeConfigFile(configFile,bufferMoveConfigIn);
}


int LaserCameraTrajServer::writeDataQueueToFile(QQueue<Pinfo> &bufferPointQueueIn,QString newFileName)
{

         //文件初始化
        char*  ch;
        QByteArray ba = newFileName.toLatin1();
        ch=ba.data();
        qDebug()<<"\n\n\n*****************************************开启:将bufferPointQueueIn的数据写入到文件功能!\n";
        FILE* fileObject ;
        fileObject = fopen( ch ,"w");
        if(fileObject!=NULL)
        {
            fclose(fileObject);
        }
        if ((fileObject=fopen(ch,"a+"))==NULL)//
        {
             qDebug()<< "文件打开失败:" <<ch;
             return -1;
        }

        if(bufferPointQueueIn.size()<1)
        {
            fclose(fileObject);
            qDebug()<< "queue里面没有数据了，无法保存到文件";
            return -1;
        }

        //文件内容填充
        for(int i=0;i<bufferPointQueueIn.size();i++)
        {
            fprintf( fileObject, "%f ,%f ,%f ," , bufferPointQueueIn[i].x,bufferPointQueueIn[i].y,
                     bufferPointQueueIn[i].z);
            fprintf( fileObject, "\n"  );
        }
        fprintf( fileObject, "\n"  );



        fflush( fileObject);
        fclose(fileObject);


        return 1;

}

int LaserCameraTrajServer::writeConfigFile(QString configFileName, BufferMoveConfig bufferMoveConfigIn)
{

    qDebug() << "LaserCameraTrajServer::writeConfigFile ==== 初始化配置文件：" << configFileName;

    DomParser domparser;
    bool ok = domparser.openXml(configFileName, QIODevice::ReadWrite);
    if( !ok )
    {

        qDebug() << " 配置文件打开失败：" << configFileName;
        return -1;
    }


    QDomNode maindomNode = domparser.findSubNode(domparser.getRootDomElement(), "bufferMoveConfig", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }


    QDomNode domNode = domparser.findSubNode(maindomNode, "allowMaxGap", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.writeXml(domNode, bufferMoveConfigIn.allowMaxGap);



    domparser.closeXml(true);
    bufferMoveConfig=bufferMoveConfigIn;
    return 1;
}

int LaserCameraTrajServer::initialConfigFile(QString configFileName)
{
    qDebug() << "LaserCameraTrajServer::initialConfigFile ==== 初始化配置文件：" << configFileName;

    DomParser domparser;
    bool ok = domparser.openXml(configFileName, QIODevice::ReadOnly);
    if( !ok )
    {

        qDebug() << " 配置文件打开失败：" << configFileName;
        return -1;
    }

    QDomNode maindomNode = domparser.findSubNode(domparser.getRootDomElement(), "bufferMoveConfig", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }


    QDomNode domNode = domparser.findSubNode(maindomNode, "allowMaxGap", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(domNode, bufferMoveConfig.allowMaxGap);

    domparser.closeXml();
    return 1;
}

int LaserCameraTrajServer::loopOnce()
{
    Pinfo tmpP;
    programMcController->mcControllerInterpreter->getCameraPointInBase(tmpP.x,tmpP.y,tmpP.z);

    usingMutex.lock();
    if(bufferPointQueue.size()>1)
    {
        if(fabs(tmpP.x-bufferPointQueue.back().x)>bufferMoveConfig.allowMaxGap
                ||fabs(tmpP.y-bufferPointQueue.back().y)>bufferMoveConfig.allowMaxGap
                ||fabs(tmpP.z-bufferPointQueue.back().z)>bufferMoveConfig.allowMaxGap)
        {
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "LaserCameraTrajServer",  12951, robotId ,bufferMoveConfig.allowMaxGap);
            usingMutex.unlock();
            return 0;
        }

        if(fabs(tmpP.x-bufferPointQueue.back().x)>D_DELTA_MOVE ||fabs(tmpP.y-bufferPointQueue.back().y)>D_DELTA_MOVE
                ||fabs(tmpP.z-bufferPointQueue.back().z)>D_DELTA_MOVE)
        {

            bufferPointQueue.enqueue(tmpP);
            if(bufferPointQueue.size()>D_MAX_QUEUE_SIZE)
            {
                bufferPointQueue.dequeue();
                qDebug()<<"bufferPointQueue exceed limit size"<<D_MAX_QUEUE_SIZE;
            }
        }
    }
    else
    {
        bufferPointQueue.enqueue(tmpP);
    }


    usingMutex.unlock();

    return 1;
}

#include <unistd.h>
#include <sys/syscall.h>
#define gettid() syscall(__NR_gettid)

void* LaserCameraTrajServer::threadRun(void *)
{
    printf("LaserCameraTrajServer theread start run !!!");

    struct sched_param param;
    param.__sched_priority = 30;
    sched_setscheduler(0,SCHED_FIFO,&param);
    qDebug()<<"************************LaserCameraTrajServer pid="<<gettid();
    //setStackSize(81920);
    int policy, priority;
    pthread_getschedparam(pthread_self(), &policy, (struct sched_param *)&priority);

    while(1)
    {
       // pthread_testcancel();/*the thread can be killed only here*/
         QThread::msleep(50);
         if(isStart)
         {
             loopOnce();
         }


    }
    printf("LaserCameraTrajServer thread quit!!!");
}

void* LaserCameraTrajServer::threadTask( void* classPtr )
{
    qDebug()<<"LaserCameraTrajServer thread run-------------------------";
    return ((LaserCameraTrajServer*)classPtr)->threadRun(NULL);
}

void LaserCameraTrajServer::createThread()
{
    pthread_t threadId;
    int ret=pthread_create( &threadId, NULL, &LaserCameraTrajServer::threadTask,this);
    qDebug()<<"LaserCameraTrajServer createThread-------------------------";
}

void LaserCameraTrajServer::addMsg(int messageLevel, string componentName,
                                       string messageType, int messageCode, int robotId, int parameter1,
                                       int parameter2, int parameter3, int parameter4, QString message)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;
   // tmpMsg.time = t1.tv_sec;


    QString infomationStr;

    switch(messageCode)
    {
    case 12955:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",P%1点为空!").arg(parameter1)+message;
    }
        break;
    case 12954:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",P%1点不能为关节点格式!").arg(parameter1)+message;
    }
        break;
    case 12953:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",波纹路径计算折点失败!").arg(parameter1)+message;
    }
        break;
    case 12952:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",laserCameraConfig.xml读取失败！").arg(parameter1)+message;
    }
        break;
    case 12951:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",点间距超过允许值%1mm！").arg(parameter1)+message;
    }
        break;


    default:
    {

    }
    }


    tmpMsg.MessageInformation = infomationStr.toStdString();

    // qDebug()<<"53444444444444444444444"<< tmpMsg.messageCode;
    MessageLog::getInstance()->addMessage(tmpMsg);
}


